﻿#include "purepursuit.h"
#include "Astar.h"
#include <qmath.h>
#include <cstdlib>
using namespace std;
purepursuit::purepursuit(btosgMyCar* myVehicle)
{
//    btVector3 pos;
//    pos = myVehicle->vehicle->getChassisWorldTransform().getOrigin();
//    carx = pos.x();
//    cary = pos.y();
//    btScalar ang1;
//    btScalar ang2;
//    btScalar ang3;
//    myVehicle->vehicle->getChassisWorldTransform().getRotation().getEulerZYX(ang1,ang2,ang3);
//    yaw = ang1*180/3.1415926535;
//    velocity = myVehicle->vehicle->getCurrentSpeedKmHour()*0.27777777;
    this->yaw = 0;
    this->s.delta = 0;
    this->myVehicle = myVehicle;
}
void purepursuit::update(btosgMyCar* myVehicle)
{
    btVector3 pos;
    pos = myVehicle->vehicle->getChassisWorldTransform().getOrigin();
    this->carx = pos.x();
    this->cary = pos.y();
    btScalar ang1;
    btScalar ang2;
    btScalar ang3;
    myVehicle->vehicle->getChassisWorldTransform().getRotation().getEulerZYX(ang1,ang2,ang3);
//    this->yaw = ang1*180/3.1415926535;
    this->yaw = this->yaw +this->velocity/this->L*tan(this->s.delta)*this->dt;
    velocity = myVehicle->vehicle->getCurrentSpeedKmHour()*0.27777777;
    std::cout<<this->yaw << std::endl;
//    std::cout<<cary << std::endl;
}
void purepursuit::PidControl(double target)
{
    if((myVehicle->vehicle->getCurrentSpeedKmHour())>target)
    {
        double a = (myVehicle->vehicle->getCurrentSpeedKmHour()-target)*800/this->dt;
        myVehicle->vehicle->applyEngineForce(-a, 2);
        myVehicle->vehicle->applyEngineForce(-a, 3);
    }else
    {
        myVehicle->vehicle->applyEngineForce(this->newton, 2);
        myVehicle->vehicle->applyEngineForce(this->newton, 3);
        cout<<this->newton<<endl;
        cout<<myVehicle->vehicle->getCurrentSpeedKmHour()*0.27777777<<endl;
    }
//     std::cout<<carx << std::endl;
//    std::cout<<cary << std::endl;
}
int purepursuit::calc_target_index(double x[],double y[])
{
//    std::cout<<"len" << std::endl;
//    int len = sizeof(&x)/sizeof(double);
//    std::cout<<sizeof(&x)<< std::endl;
    int len = pointNum;
//    std::cout<<len<< std::endl;
    double dx[1000],dy[1000],d[1000];
//    double dx[1000000],dy[1000000],d[1000000];
    for(int i = 0;i < len;i++)
    {
        dx[i] = this->carx - x[i];
        dy[i] = this->cary - y[i];
        d[i] = sqrt(qPow(dx[i],2) + qPow(dy[i],2));
//        std::cout<<d[i]<< std::endl;
    }
    double minvalue = *std::min_element(d + 0,d + len );
//     std::cout<<minvalue<< std::endl;
    int index;
    for(int i = 0;i < len;i++)
    {
        if((d[i] - minvalue) < 1e-6)
        {
        index = i;
        break;
        }
    }
//    double N = 0.0;
    double Lf = this->k*this->velocity + this->Lfc;
//    int fin = index + 1;

    double nL = 0;
    while(Lf > nL && (index + 1) < len)
    {
        double nx = x[index + 1] - x[index];
        double ny = y[index + 1] - y[index];
        nL += sqrt(qPow(nx,2) + qPow(ny,2));
        index += 1;
    }
//    std::cout<<index<< std::endl;
    return index;
}
purepursuit::solutions purepursuit::pure_pursuit_control(double x[],double y[],int pind)
{
    solutions s;
    double tx,ty;
//    int len = sizeof(x)/sizeof(double);
    unsigned long long len = pointNum ;
    int ind = calc_target_index(x,y);
//    std::cout<< ind <<endl;
//    std::cout<< pind <<endl;
    if(pind >= ind)
        ind = pind;
    if(ind < len)
    {
        tx = x[ind];
        ty = y[ind];
//        std::cout<< ind <<endl;
    }else
    {
        tx = x[len - 1];
        ty = y[len - 1];
        ind = len - 1;
    }
    double alpha = atan2(ty - this->cary,tx - this->carx) - this->yaw;
    double Lf = this->k*this->velocity + this->Lfc;
    double delta = atan2(2.0*this->L*sin(alpha)/Lf,1.0);
    s.delta = delta;
    s.ind = ind;
    return s;
}
void purepursuit::start_pursuit(double x[],double y[],btosgMyCar* myVehicle)
{
    this->update(myVehicle);
    this->PidControl(this->target_speed);
//    std::cout<<"len" << std::endl;
//      target_ind = calc_target_index(x,y);
//    std::cout<< target_ind <<std::endl;
//    double rad = 0;
//    solutions s;
    this->s = pure_pursuit_control(x,y,target_ind);
//    std::cout<< pointNum - 1 <<std::endl;
    std::cout<<"s.ind  "<< s.ind <<std::endl;
    if((pointNum - 1) <= s.ind)
    {
        myVehicle->vehicle->setSteeringValue(btScalar(0), 0);
        myVehicle->vehicle->setSteeringValue(btScalar(0), 1);
        myVehicle->vehicle->setBrake(10000000,2);
        myVehicle->vehicle->setBrake(10000000,3);
        end = 0;
        std::cout<< "end" <<std::endl;
    }else {
        btScalar ang1 = atan(2.0*this->A/(2.0*this->A/(tan(s.delta) - this->L)));
        btScalar ang2 = atan(2.0*this->A/(2.0*this->A/(tan(s.delta) + this->L)));
//                std::cout<< ang1 <<std::endl;
//        myVehicle->vehicle->setSteeringValue(btScalar(ang1), 0);
//        myVehicle->vehicle->setSteeringValue(btScalar(ang2), 1);
//        if(s.delta > 0)
//            s.delta = qAbs(1-s.delta);
//        else
//            s.delta = qAbs(1-qAbs(s.delta));

//        if(s.delta<-1.3){
//            myVehicle->vehicle->setSteeringValue(btScalar(-1.3), 0);
//            myVehicle->vehicle->setSteeringValue(btScalar(-1.3), 1);

//            std::cout<<"s.delta  "<< -1.3 <<std::endl;
//        }else if(s.delta>1.3){
//            myVehicle->vehicle->setSteeringValue(btScalar(1.3), 0);
//            myVehicle->vehicle->setSteeringValue(btScalar(1.3), 1);

//            std::cout<<"s.delta  "<< 1.3 <<std::endl;
//        }else{
            if(beforeDelta==-20){
                beforeDelta=s.delta;
            }else{
//                if(abs(beforeDelta-s.delta)>0.4){
//                    s.delta=(s.delta+beforeDelta)/2;

//                }else if(abs(beforeDelta-s.delta)>0.7){
//                    s.delta=s.delta/3+beforeDelta*2/3;
//                }else if(abs(beforeDelta-s.delta)>1){
//                    s.delta=s.delta/4+beforeDelta*3/4;
//                }else{
//                    s.delta=s.delta/5+beforeDelta*4/5;
//                }
//                s.delta=s.delta/5+beforeDelta*4/5;

                if(abs(beforeDelta-s.delta)<0.7){
                    s.delta=s.delta*2/3+beforeDelta/3;
                    beforeDelta=s.delta;
                }else{
                    s.delta=s.delta*2/5+beforeDelta*3/5;
//                    s.delta=beforeDelta;
                    beforeDelta=s.delta;
                }




            }


            myVehicle->vehicle->setSteeringValue(btScalar(s.delta), 0);
            myVehicle->vehicle->setSteeringValue(btScalar(s.delta), 1);
            std::cout<<"s.delta  "<< s.delta <<std::endl;

//           }

            //这里添加的是动力约束（防止有一些地方翻越不了）
//            myVehicle->addPower();
//            myVehicle->addPower();
            int engine=40;
            if(beforeInd>=s.ind){
                beforeCount++;

                if(beforeCount<20){
                    myVehicle->setPower(47);
                    engine=40;
                }else if(beforeCount>=20&&beforeCount<30){
                    myVehicle->setPower(58);
                    engine=70;
                    std::cout<<"houhouhouhouhouhou beforeCount="<<beforeCount <<std::endl;
                }else if(beforeCount>=30&&beforeCount<40){
                    myVehicle->setPower(67);
                    engine=100;
                    std::cout<<"houhouhouhouhouhou beforeCount="<<beforeCount <<std::endl;
                }else if(beforeCount>=40&&beforeCount<55){

//                    myVehicle->addPower();
                    myVehicle->setPower(71);

                    engine=300;
                    std::cout<<"bubububububububu beforeCount="<<beforeCount <<std::endl;
                }
                else if(beforeCount>=55&&beforeCount<70){
                    myVehicle->setPower(76);
                    engine=600;
                    std::cout<<"gegegegegegegegege beforeCount="<<beforeCount <<std::endl;
                }else if(beforeCount>=70&&beforeCount<85){
                    myVehicle->setPower(81);
//                    myVehicle->addPower();
//                    myVehicle->addPower();
                    engine=999;
                    std::cout<<"yinyinyinyin beforeCount="<<beforeCount <<std::endl;
                }else if(beforeCount>=85&&beforeCount<100){
                    myVehicle->setPower(88);
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
                    engine=1500;
                    std::cout<<"hehehehehehehehehehe beforeCount="<<beforeCount <<std::endl;
                }
                else if(beforeCount>100&&beforeCount<110){
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
                    myVehicle->setPower(93);
                    engine=3000;
                    std::cout<<"hahahahahhahahahahahahah beforeCount="<<beforeCount <<std::endl;
                }else if(beforeCount>=110&&beforeCount<130){
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
                    myVehicle->setPower(98);
                    engine=10000;
                    std::cout<<"hahahahahhahahahahahahah beforeCount="<<beforeCount <<std::endl;
                    if(beforeCount>=110){
                        beforeCount=80;
                    }

                }else if(beforeCount>=130){
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
//                    myVehicle->addPower();
                    myVehicle->setPower(100);
                    engine=20000;
                    std::cout<<"hahahahahhahahahahahahah beforeCount="<<beforeCount <<std::endl;
                    if(beforeCount>=135){
                        beforeCount=80;
                    }
                }

            }else{
                myVehicle->setPower(28);
                beforeCount=0;
                beforeInd=s.ind;
                std::cout<<"didididiidididiidiid beforeCount="<<beforeCount <<std::endl;
            }
            myVehicle->vehicle->applyEngineForce(engine,0);
            myVehicle->vehicle->applyEngineForce(engine,1);
            myVehicle->vehicle->applyEngineForce(engine,2);
            myVehicle->vehicle->applyEngineForce(engine,3);





    }

//    myVehicle->vehicle->setSteeringValue(btScalar(rad), 0);
//    myVehicle->vehicle->setSteeringValue(btScalar(rad), 1);
}
//绘制轨迹线条
osg::ref_ptr<osg::Node> purepursuit::CreateLine(double node[][3], int nodeNum, float lineWidth)
{
    osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;

    //设置线宽
    osg::ref_ptr<osg::LineWidth> lineSize = new osg::LineWidth;
    lineSize->setWidth(lineWidth);
    geom->getOrCreateStateSet()->setAttributeAndModes(lineSize.get(), osg::StateAttribute::ON);
    geom->getOrCreateStateSet()->setMode(GL_LIGHTING, osg::StateAttribute::OFF);

    //定义node数组
    osg::ref_ptr<osg::Vec3Array> v = new osg::Vec3Array;
    geom->setVertexArray(v.get());
    int nodeCount = 0;
    while (nodeCount < nodeNum)
    {
//        v->push_back(osg::Vec3(node[nodeCount][0], node[nodeCount][1], node[nodeCount][2]));

        v->push_back(osg::Vec3(node[nodeCount][0], node[nodeCount][1], node[nodeCount][2]));
        std::cout << "Point " << nodeCount + 1 << ": "
                  << node[nodeCount][0] << ", "
                  << node[nodeCount][1] << ", "
                  << node[nodeCount][2] << std::endl;
        nodeCount++;
    }
    //定义颜色数组
    osg::ref_ptr<osg::Vec4Array> c = new osg::Vec4Array;
    geom->setColorArray(c.get());
    geom->setColorBinding(osg::Geometry::BIND_OVERALL);
    // 4个参数：RGBA，A-alpha通道，一般用作不透明度参数，0~完全透明（看不见），1~传统的数字图像
    c->push_back(OSG_COLOR_GRAY);

    //设置顶点关联方式
    geom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINE_STRIP, 0, nodeNum));

    //几何组结点
    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
    geode->osg::Geode::addDrawable(geom.get());
    return geode.get();
}
//void purepursuit::UpdateTraj(double *PointArray, int PointNum)
//{
//    m_PointNum = PointNum;
//    if (m_PointNum > 10000)
//    {

//        PointArray = PointArray + (m_PointNum - 10000)*3;
//        m_PointNum = 10000;
//    }

//    //当PointArray==nullptr或PointNum==0时删除轨迹
//    if (PointArray == nullptr || PointNum == 0 || PointNum < 0)
//    {
//        m_PointNum = 0;
//        pTrajectory_install->GetValidGroup()->removeChild(0,1);
//    }
//    else
//    {
        //复制轨迹点到缓存数组
//        memcpy(m_PointArray, PointArray, m_PointNum*3*sizeof(double));
        //创建轨迹线条并显示
//        pTrajectory_install->GetValidGroup()->removeChild(0,1);
//        pTrajectory_install->GetValidGroup()->addChild(CreateLine(m_PointArray, m_PointNum));
//    }
//}
